#include "hardwareDef.h"

#include "SBUS.h"
SBUS sbus(Serial);

#include "RCPWM.h"
#include "IMU.h"
#include "simpleMath.h"
#include "ERR.h"
#include "PID.h"
#include "MIXER.h"

uint8_t flightMode;//飞行模式，0失控或自稳模式，1手动模式，2攻角模式，3攻角速率双闭环模式，4半自稳模式
uint8_t modePre;//飞行模式预装载，如果没有问题才会覆盖飞行模式，0失控或自稳模式，1手动模式，2攻角模式，3攻角速率双闭环模式，4半自稳模式
float input[6] = {};//输入数据，横滚0、俯仰1、油门2、偏航3、直接力4、舵面来流指向5，归一法
float feedback[6] = {};//横滚速率0、俯仰速率1、偏航速率2、AOA 3、横滚角4、俯仰角5，归一法
float ctrl[6] = {};//控制数据，横滚0、俯仰1、油门2、偏航3、直接力4、舵面来流指向5，归一法
int RCPWM[chNum] = {};//RCPWM的数据数组，所有飞控的输出数据直接放这个数组就可以了
int RC[8] = {};//RC数据，横滚0、俯仰1、油门2、偏航3、自稳4、模式5、直接力6、舵面来流指向7
float trueAOA;

unsigned long tStamp[3] = {};//用于收集各传感器更新事件戳，以此判断是否失效,0遥控，1攻角，2陀螺仪

void setup()
{
  sbus.begin(false);

  INIT_AOA();
  sei();//打开全局中断
  INIT_SERVO();//初始化servo信号
  INIT_CALCTIM();//初始化计算定时器
}

void INIT_CALCTIM()
{
  TCCR2A = 0b00000000;
  TCCR2B = 0b00000111;  
  TCNT2 = 127;
}

ISR(TIMER2_OVF_vect)
{
  TIMSK2 = 0;
  ERR();
  PID();
  MIXER();
}

void loop()
{
  if (Serial.available())
  {
    tStamp[0] = millis();
  }
  sbus.process();//处理串口的SBUS信号
  for (uint8_t i = 0; i < 8; i ++)
  {
    RC[i] = map(sbus.getChannel(i + 1),221,1824,988,2012);
  }
  CMD();
  RX_AOA();
  RX_GYRO();
}

void CMD()
{
  input[0] = mapF(RC[0],988,2012,-1,1);//横滚
  input[1] = mapF(RC[1],988,2012,-1,1);//俯仰
  input[2] = mapF(RC[2],988,2012,-1,1);//油门
  input[3] = mapF(RC[3],988,2012,-1,1);//偏航
  input[4] = mapF(RC[6],988,2012,-1,0);//直接力控制
  if (RC[7] > 1500)
  {//舵面来流指向开关
    input[5] = 0;
  }
  else
  {
    input[5] = 1;
  }
  //飞行模式检测，先执行半自稳判断，最后执行自稳判断，自稳最后覆盖掉前面的
  if (RC[5] < 1300)
  {//飞行模式，0失控或自稳模式，1手动模式，2攻角模式，3攻角速率双闭环模式，4半自稳模式
    modePre = 1;
  }
  else if (RC[5] < 1700)
  {
    modePre = 2;
  }
  else
  {
    modePre = 3;
  }
  if (RC[4] > 1500)
  {//自稳开关
    modePre = 0;//大于1500进入自稳
  }
}

void TX()
{//发送单元，用于黑匣子记录，100kbit/s，8位数据，2停止位，无奇偶校验
  static unsigned long t2;
  if (millis() - t2 >= 50)
  {
    t2 = millis();
    Serial.print(millis());
    Serial.print(",");
    Serial.print(input[0]);
    Serial.print(",");
    Serial.print(feedback[0]);
    Serial.print(",");
    Serial.print(input[1]);
    Serial.print(",");
    Serial.print(feedback[1]);
    Serial.print(",");
    Serial.print(feedback[3]);
    Serial.print(",");
    Serial.print(flightMode);
    Serial.println(",");
  }
}
